/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  State machine
 */


#ifndef STATE_MACHINE_H_
#define STATE_MACHINE_H_

#include <iostream>

#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include "std_msgs/Bool.h"

#include "../motionCtrl/motionCtrl.h"
#include "../servoCtrl/servoCtrl.h"
#include "group6/motion_ctrl.h"
#include "group6/approach.h"
#include "group6/transformManipulatorServo.h"
#include "group6/transformCameraServo.h"

#include "../pathPlanner/pathPlanner.h"
#include "group6/goToNextGoal.h"

#define MAX_RAISE_TRIES 3

class StateMachine
{
public:

    enum states {
        STATE_INITIALIZING = 0,
        STATE_MOVE_TO_TARGET = 1, 
        STATE_APPROACH_TARGET = 2,
        STATE_ACQUIRE_TARGET = 3,
        STATE_MOVE_TO_BASKET = 4,
        STATE_FINISHED = 5,
        STATE_TARGET_DROPPED = 6,
        STATE_APPROACH_BASKET = 7,
        STATE_BACKOFF_FROM_BASKET = 8,
        STATE_BACKOFF_FROM_TOY = 9,
        STATE_EMERGENCY_BACKOFF = 10,
        STATE_MISSION_COMPLETE = 11,
        STATE_RANDOM_SEARCH = 12,
	STATE_BACKOFF_RANDOM_SEARCH = 13
    };

    /*const char* stateDescriptions[] = 
    {
        "Initializing",
        "Moving to target",
        "Approaching target",
        "Acquiring target",
        "Depositing target",
        "Finished",
        "ERROR! Target dropped!",
        "Aproaching basket",
        "Backing off from basket"
    };*/

    StateMachine(ros::NodeHandle& nh);
    ~StateMachine(){}

    void start();

    void updateMotionCtrlState(const std_msgs::Int16::ConstPtr& state);

private:

    int commandToPathPlanner(PathPlanner::currentDestination target);

    bool approachCommandToMotionCtrl(PathPlanner::currentDestination target);

    bool lowerManipulator();
    bool hoverManipulator();
    bool raiseManipulator();
    bool grab();
    bool release();
    bool initCameraAttitude();
    bool transformManipulator(int servoType, int action);
    bool transformCamera(int servoType, int action, int rotation);

    bool testNodes();
    bool escapeCommandToMotionCtrl(int target);

    void updateState(states newState);
    void rosSleep();

    ros::NodeHandle mNodeHandle;

    // state manager
    enum states currentState;

    //MotionCtrl
    ros::Publisher mMotionCtrlApproachPub;
    ros::ServiceClient mApproachClient;
    group6::approach mServiceToMotionCtrl;
    group6::motion_ctrl mInitApproachMsg;

    //PathPlanner
    ros::ServiceClient mGoToGoalClient;
    group6::goToNextGoal mServiceToPathPlanner;

    //ServoCtrl
    ros::ServiceClient ptuServoClient;
    ros::ServiceClient manipServoClient;
    group6::transformManipulatorServo mTransformManipulator;
    group6::transformCameraServo mTransformCamera;

    //debug
    double debugON;

};

#endif

